from system.action import Action
from system.vec2 import Vec2
import time

class Body(Action):
    def run(self, **args):
        fpsTime = args.get('fpsTime',5)
        fpsStart = time.time()
        numFrames = 0
        
        lastaforce = Vec2(0,0)
        
        for atime, dtime in self.enumerateTime(args.get("updateTime",0.01)):
            numFrames += 1
            currentTime = time.time()
            if currentTime - fpsStart > fpsTime:
                self.log(float(numFrames) / (currentTime - fpsStart))
                fpsStart = currentTime
                numFrames = 0
            
            mass = self.object.getVar("mass",1)
            
            force = Vec2(self.object.getVar("force",(0,0)))
            aforce = Vec2(self.object.getVar("alignedForce",(0,0)))
            torque = self.object.getVar("torque",0)
            
            if (aforce - lastaforce).length() > 0.01:
                self.log("aforce",aforce,lastaforce)
                lastaforce = aforce
            
            friction = self.object.getVar("friction",0.5)
            rotFriction = self.object.getVar("rotFriction",0.5)
            rot = self.object.getVar("rot",0)
            
            totalForce = force + aforce.rotate(rot)
            acc = totalForce / mass
            vel = (Vec2(self.object.getVar("vel",(0,0))) + acc * dtime) * (1 - friction * dtime)
            pos = Vec2(self.object.getVar("pos",(0,0))) + vel * dtime
            
            rotAcc = torque / mass
            rotVel = (self.object.getVar("rotVel",0) + rotAcc * dtime) * (1 - rotFriction * dtime)
            rot = rot + rotVel * dtime
            
            self.object.setVar("vel",vel.toTuple())
            self.object.setVar("pos",pos.toTuple())
            self.object.setVar("rotVel",rotVel)
            self.object.setVar("rot",rot)
